{
  "cells": [
    {
      "cell_type": "markdown",
      "id": "cc72e99e",
      "metadata": {},
      "source": [
        "# Stereo Visual Odometry with GTSAM\n",
        "\n",
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/StereoVOExample.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
        "\n",
        "This notebook demonstrates a simple 3D stereo visual odometry problem using the GTSAM Python wrapper. The scenario is as follows:\n",
        "\n",
        "1.  A robot starts at the origin (Pose 1: `X(1)`).\n",
        "2.  The robot moves approximately one meter forward (Pose 2: `X(2)`).\n",
        "3.  From both poses, the robot observes three landmarks (`L(1)`, `L(2)`, `L(3)`) with a stereo camera.\n",
        "\n",
        "We will:\n",
        "- Define camera calibration and noise models.\n",
        "- Create a factor graph representing the problem.\n",
        "- Provide initial estimates for poses and landmark positions.\n",
        "- Optimize the graph using Levenberg-Marquardt to find the most probable configuration.\n",
        "\n",
        "**Note:** This example is also available in C++. If you're interested in the C++ implementation, refer to `StereoVOExample.cpp` in the [GTSAM examples directory](https://github.com/borglab/gtsam/tree/develop/examples)."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "e33e553a",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2025, Georgia Tech Research Corporation,\n",
        "Atlanta, Georgia 30332-0415\n",
        "All Rights Reserved\n",
        "\n",
        "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
        "\n",
        "See LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "id": "43bbbfe4",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "id": "778ceb66",
      "metadata": {
        "tags": [
          "hide-cell"
        ]
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "\n",
        "# For shorthand for common GTSAM types (like X for poses, L for landmarks)\n",
        "from gtsam.symbol_shorthand import X, L"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "8d3c4fd5",
      "metadata": {},
      "source": [
        "## 1. Setup Factor Graph and Priors\n",
        "\n",
        "We start by creating an empty `NonlinearFactorGraph`. The first pose `X(1)` is assumed to be at the world origin. To reflect this, we add a hard constraint using `NonlinearEqualityPose3`, which fixes `X(1)` to the identity pose. This ensures that the optimization has a known reference point for the rest of the variables. In GTSAM, factor keys are typically integers, and `X(1)` is a symbolic shorthand for such a key.\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "id": "342a585f",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Graph size after adding prior: 1\n"
          ]
        }
      ],
      "source": [
        "# Create an empty nonlinear factor graph\n",
        "graph = gtsam.NonlinearFactorGraph()\n",
        "\n",
        "# Define the first pose at the origin\n",
        "first_pose = gtsam.Pose3() # Default constructor is identity\n",
        "\n",
        "# Add a prior constraint on X(1)\n",
        "graph.add(gtsam.NonlinearEqualityPose3(X(1), first_pose))\n",
        "\n",
        "print(\"Graph size after adding prior:\", graph.size())\n"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "4cbc48f7",
      "metadata": {},
      "source": [
        "## 2. Define Camera Calibration and Measurement Noise\n",
        "\n",
        "To accurately model the stereo observations, we must define both the measurement noise model and the intrinsic calibration of the stereo camera system.\n",
        "- **Measurement Noise:** We assume an isotropic Gaussian noise model for the stereo measurements. Since each observation consists of three pixel values $(u_L, u_R, v)$, we use `Isotropic.Sigma(3, 1.0)` to define a noise model with unit standard deviation in each of the three dimensions.\n",
        "- **Camera Calibration ([`Cal3_S2Stereo`](/gtsam/geometry/doc/Cal3_S2Stereo.ipynb)):** We define a stereo calibration model with the following properties:\n",
        "  - Focal lengths: $f_x=f_y=1000$\n",
        "  - Skew: $s=0$\n",
        "  - Principal point: $(u,v)=(320,\\,240)$\n",
        "  - Baseline: $b=0.2$\n",
        "  \n",
        "These parameters represent a synthetic stereo camera setup for this minimal example."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "id": "e04b26c7",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "K: 1000    0  320\n",
            "   0 1000  240\n",
            "   0    0    1\n",
            "Baseline: 0.2\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Create stereo camera calibration object\n",
        "K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)\n",
        "print(K)\n",
        "\n",
        "# Define the stereo measurement noise model (isotropic, 1 pixel standard deviation)\n",
        "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "a2f867ed",
      "metadata": {},
      "source": [
        "## 3. Add Stereo Factors\n",
        "\n",
        "We now add stereo factors to the graph. Each factor connects a camera pose to a landmark.\n",
        "The `GenericStereoFactor3D` takes:\n",
        "*   `measured`: The `StereoPoint2` measurement $(u_L, u_R, v)$.\n",
        "*   `model`: The noise model for the pixel measurement.\n",
        "*   `poseKey`: Key for the camera pose.\n",
        "*   `landmarkKey`: Key for the landmark.\n",
        "*   `K`: The stereo camera calibration.\n",
        "\n",
        "We'll use `L(1)`, `L(2)`, `L(3)` to represent our three landmarks."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "id": "e9778bec",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Graph size after adding X(1) factors: 4\n"
          ]
        }
      ],
      "source": [
        "# Factors from X(1) to landmarks\n",
        "graph.add(gtsam.GenericStereoFactor3D(\n",
        "    gtsam.StereoPoint2(520, 480, 440), measurement_noise, X(1), L(1), K\n",
        "))\n",
        "graph.add(gtsam.GenericStereoFactor3D(\n",
        "    gtsam.StereoPoint2(120, 80, 440), measurement_noise, X(1), L(2), K\n",
        "))\n",
        "graph.add(gtsam.GenericStereoFactor3D(\n",
        "    gtsam.StereoPoint2(320, 280, 140), measurement_noise, X(1), L(3), K\n",
        "))\n",
        "\n",
        "print(\"Graph size after adding X(1) factors:\", graph.size())\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "id": "495fc44f",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Graph size after adding all factors: 7\n"
          ]
        }
      ],
      "source": [
        "# Factors from X(2) to landmarks\n",
        "graph.add(gtsam.GenericStereoFactor3D(\n",
        "    gtsam.StereoPoint2(570, 520, 490), measurement_noise, X(2), L(1), K\n",
        "))\n",
        "graph.add(gtsam.GenericStereoFactor3D(\n",
        "    gtsam.StereoPoint2(70, 20, 490), measurement_noise, X(2), L(2), K\n",
        "))\n",
        "graph.add(gtsam.GenericStereoFactor3D(\n",
        "    gtsam.StereoPoint2(320, 270, 115), measurement_noise, X(2), L(3), K\n",
        "))\n",
        "\n",
        "print(\"Graph size after adding all factors:\", graph.size())"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "b6dd7e48",
      "metadata": {},
      "source": [
        "## 4. Create Initial Estimates\n",
        "\n",
        "Nonlinear optimization requires initial estimates for all variables (poses and landmarks). We create a `Values` object named `initial_estimate` to store these. To see the effects of optimization in the later sections, we introduce a small error when inserting the initial estimate of pose `X(2)`. In summary, this is what we insert into `initial_estimate`:\n",
        "*   Pose `X(1)`: Identity pose at the origin.\n",
        "*   Pose `X(2)`: Camera pose after the robot moves forward by one meter, slightly off-axis and with a small overshoot.\n",
        "*   Landmarks (`L(1)`, `L(2)`, `L(3)`): Placed at estimated 3D positions.\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "id": "b4be6ab2",
      "metadata": {},
      "outputs": [],
      "source": [
        "initial_estimate = gtsam.Values()\n",
        "\n",
        "# Initial estimate for X(1)\n",
        "initial_estimate.insert(X(1), first_pose)\n",
        "\n",
        "# Initial estimate for X(2) - robot moved forward ~1m, with some small error\n",
        "# Pose3(rotation, translation)\n",
        "# gtsam.Rot3() is identity rotation\n",
        "initial_pose2 = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1, -0.1, 1.1))\n",
        "initial_estimate.insert(X(2), initial_pose2)\n",
        "\n",
        "# Initial estimates for landmark positions (Point3)\n",
        "initial_estimate.insert(L(1), gtsam.Point3(1.0, 1.0, 5.0))\n",
        "initial_estimate.insert(L(2), gtsam.Point3(-1.0, 1.0, 5.0))\n",
        "initial_estimate.insert(L(3), gtsam.Point3(0.0, -0.5, 5.0))\n"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "1c1a5a73",
      "metadata": {},
      "source": [
        "## 5. Optimize the Factor Graph\n",
        "\n",
        "We use the Levenberg-Marquardt optimizer to find the solution that minimizes the sum of squared errors defined by the factors in the graph, starting from the `initial_estimate`.\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "id": "d0c3a87b",
      "metadata": {},
      "outputs": [],
      "source": [
        "# Create a Levenberg-Marquardt optimizer\n",
        "optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate)\n",
        "\n",
        "# Optimize the graph\n",
        "result = optimizer.optimize()"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "3b3685e1",
      "metadata": {},
      "source": [
        "## 6. Analyze Results\n",
        "\n",
        "After optimization, we extract the refined poses and landmark positions from the `result` returned by the optimizer.\n",
        "\n",
        "Recall that in this example, the robot begins at the origin, and we expect the second pose `X(2)` to be apprixmately one meter forward along the Z-axis, so `X(2)` should ideally be near `Pose3(Rot3(), Point3(0, 0, 1.0))`.\n",
        "\n",
        "GTSAM minimizes the total reprojection error (i.e. the discrepancy between observed and predicted stereo image coordinates) across all factors. We can evaluate this using `graph.error()` to compute the total error before and after optimization. A significant drop in this error indicates that the optimizer successfully refined the estimates.\n",
        "\n",
        "To better understand the outcome of this example, we print the error, both prior- and post-optimization, along with the optimized poses and landmark positions. This helps verify:\n",
        "- That the optimizer is successful in refining the estimates.\n",
        "- That the second pose `X(2)` aligns with the expected forward motion.\n",
        "- That the landmark locations are consistent and reasonable."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "id": "093fe3dc",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Initial Error: 3434.6236\n",
            "Final Error  : 0.0000\n",
            "\n",
            "Optimized Pose X(1):\n",
            " R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0 0 0\n",
            "\n",
            "\n",
            "Optimized Pose X(2):\n",
            " R: [\n",
            "\t1, 4.90433e-17, -3.66864e-16;\n",
            "\t-4.90433e-17, 1, 1.63701e-15;\n",
            "\t3.66864e-16, -1.63701e-15, 1\n",
            "]\n",
            "t:  6.11736e-13 -6.16862e-13            1\n",
            "\n",
            "Translation component of X(2): [ 6.11735920e-13 -6.16861807e-13  1.00000000e+00]\n",
            "\n",
            "Optimized Landmark (L(1)):\n",
            " [1. 1. 5.]\n",
            "\n",
            "Optimized Landmark (L(2)):\n",
            " [-1.  1.  5.]\n",
            "\n",
            "Optimized Landmark (L(3)):\n",
            " [-1.66287119e-16 -5.00000000e-01  5.00000000e+00]\n"
          ]
        }
      ],
      "source": [
        "print(f\"Initial Error: {graph.error(initial_estimate):.4f}\")\n",
        "print(f\"Final Error  : {graph.error(result):.4f}\")\n",
        "\n",
        "# Extract and print optimized values for clarity\n",
        "optimized_pose1 = result.atPose3(X(1))\n",
        "optimized_pose2 = result.atPose3(X(2))\n",
        "optimized_lm1 = result.atPoint3(L(1))\n",
        "optimized_lm2 = result.atPoint3(L(2))\n",
        "optimized_lm3 = result.atPoint3(L(3))\n",
        "\n",
        "print(\"\\nOptimized Pose X(1):\\n\", optimized_pose1)\n",
        "print(\"\\nOptimized Pose X(2):\\n\", optimized_pose2)\n",
        "print(f\"Translation component of X(2): {optimized_pose2.translation()}\")\n",
        "\n",
        "print(\"\\nOptimized Landmark (L(1)):\\n\", optimized_lm1)\n",
        "print(\"\\nOptimized Landmark (L(2)):\\n\", optimized_lm2)\n",
        "print(\"\\nOptimized Landmark (L(3)):\\n\", optimized_lm3)"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
